"""  
    需求：提交两个整形数据，并处理响应结果
    流程：
        前提：main函数中需要判断提交的参数是否正确。
        1.导包；
        2.初始化ROS2客户端；
        3.自定义节点类；
            3-1.创建客户端；
            3-2.等待服务连接；
            3-3.组织请求数据并发送；            
        4.调用spain函数，并传入节点对象；
            需要调用连接服务的函数，根据连接结果做下一步处理；
            连接服务后
            再处理响应结果
        5.资源释放。


"""
# 1.导包；
import sys
import rclpy
from rclpy.node import Node
from ardupilot_msgs.srv import ArmMotors #这是一个自定义的 ROS 2 服务消息类型（在 ardupilot_msgs 包中定义），用于启动或停止电机。该服务的请求消息包含一个 arm 字段，通常是一个布尔值，指示是否启动电机。
from std_srvs.srv import SetBool #这是 ROS 中标准的服务类型之一，通常用于启用或禁用某些功能。
import time #用于控制重试之间的时间间隔（例如，等待 5 秒后重试）。

# 3.自定义节点类；
class ArmMotorsClient(Node):
    def __init__(self):
        super().__init__('arm_motors_client')#传入节点名称
        self.get_logger().info("客户端arm已创建！(python)")
        # 3-1.创建客户端；
        self.client = self.create_client(ArmMotors, '/ap/arm_motors')
        # 3-2.等待服务连接；
        while not self.client.wait_for_service(timeout_sec=3.0):
            self.get_logger().info('Arm Service not available, waiting again...')

    def send_request(self):#这是自定义封装的一个函数
        #Request 是 ArmMotors 类中的一个内部类（在很多 ROS 2 的自定义服务消息类型定义中会采用这种结构方式），专门用于构建服务请求对象。
        request = ArmMotors.Request()#ArmMotors.Request() 这种语法就是在实例化 ArmMotors 这个服务消息类型中用于表示请求的内部类
        request.arm = True
        return self.client.call_async(request)#可以传入一个请求对象

def main():
    # 2.初始化ROS2客户端；
    rclpy.init()
    arm = ArmMotorsClient()

    # 进行重试，直到服务成功
    success = False
    while not success:
        future = arm.send_request()
        #处理响应，负责接收服务端响应回来的结果
        rclpy.spin_until_future_complete(arm, future)

        if future.result() is not None and future.result().result:#后面的result是自定义消息里面的一个数据类型，实际上是response的
            arm.get_logger().info('Arm Motors service call succeeded.')
            success = True
        else:
            arm.get_logger().warn('Arm Motors service call failed, retrying...')
            time.sleep(5)  # 等待 5 秒后重试

    arm.get_logger().info('Arm motors armed successfully.')
    # 4.调用spain函数，并传入节点对象；
    # rclpy.spin(MyNode())
    # client = ArmMotorsClient()
    # 5.资源释放。
    rclpy.shutdown()

if __name__ == '__main__':
    main()